#include "CarritoFisica.h"
#include <iostream>

CarritoFisica::CarritoFisica(b2World* world, float x, float y, float ancho, float alto, float angulo, string tipo):  EntidadFisica(tipo) {

	this->definicionCuerpoFisico->position.Set(x, y);
	this->definicionCuerpoFisico->angle = angulo;
	this->definicionCuerpoFisico->type=b2_dynamicBody;
	this->cuerpoFisico = world->CreateBody(definicionCuerpoFisico);


	b2PolygonShape rectangulo;
	b2Vec2 vec[8];

	vec[0] = b2Vec2(-ancho/2*69/94, alto/2);
	vec[1] = b2Vec2(-ancho/2*69/94, -alto/2*3/64);
	vec[2] = b2Vec2(ancho/2*347/376, -alto/2*3/64);
	vec[3] = b2Vec2(ancho/2*347/376, alto/2);
	rectangulo.Set(vec,4);

	this->fixture = new b2FixtureDef();
	this->fixture->shape = &rectangulo;
	this->fixture->density = 7.0f;
	this->cuerpoFisico->CreateFixture(this->fixture);

	double anguloRot = this->cuerpoFisico->GetAngle();
	float x_rueda1 = x-ancho*1/4;
	float x_rueda2 = x+ancho*117/188;
	float y_ruedas = y - alto*65/192 -40/197*alto;

	cout<<"posiciones iniciales: x1: "<<x_rueda1<<" x2: "<<x_rueda2<<" y: "<<y_ruedas<<endl;

	float x_rueda1_rotado = (-ancho*1/4)*cos(anguloRot) - (-alto*65/192 - 40/197*alto)*sin(anguloRot) + x;
	float x_rueda2_rotado = (ancho*117/188) * cos(anguloRot) - (-alto*65/192 - 40/197*alto)*sin(anguloRot) + x;
	float y_rueda1_rotado = (-ancho*1/4)*sin(anguloRot) + (-alto*75/192 - 40/197*alto)*cos(anguloRot) ;
	float y_rueda2_rotado = (ancho*117/188)*sin(anguloRot) + (-alto*75/192 - 40/197*alto)*cos(anguloRot) ;

	cout<<"posiciones finales: x1: "<<x_rueda1_rotado<<" x2: "<<x_rueda2_rotado<<" y1: "<<y- y_rueda1_rotado<<" y2: "<<y-y_rueda2_rotado<<endl;

	this->ruedaIzq = new RuedaFisica(world, x_rueda1_rotado, y+ y_rueda1_rotado, alto*80/197, alto*80/197, "rueda");
	this->ruedaDer = new RuedaFisica(world, x_rueda2_rotado, y+y_rueda2_rotado, alto*80/197, alto*80/197, "rueda");

	b2WheelJointDef jd;
	b2Vec2 eje1(0.0f, 1.0f); //ver esto

	jd.Initialize(this->cuerpoFisico, this->ruedaIzq->obtenerCuerpo(), this->ruedaIzq->obtenerCuerpo()->GetPosition(), eje1);
	jd.motorSpeed = 20.0f;
	jd.enableMotor = false;
	jd.frequencyHz = 1.5f;
	jd.dampingRatio = 0.2f;
	this->joint_ruedaDer = (b2WheelJoint*)world->CreateJoint(&jd);
	b2Vec2 eje2(0.0f,  1.0f);

	jd.Initialize(this->cuerpoFisico, this->ruedaDer->obtenerCuerpo(), this->ruedaDer->obtenerCuerpo()->GetPosition(), eje2);
	jd.motorSpeed = 20.0f;
	jd.enableMotor = false;
	jd.frequencyHz = 2.0f;
	jd.dampingRatio = 0.7f;
	this->joint_ruedaIzq = (b2WheelJoint*)world->CreateJoint(&jd);


	//agrego los extremos que sirven para poder atar sogas por los costados.

	double x1 = x + ancho/2 - 0.5f ;
	double y1 = y;

	double x2 = x - ancho/2 - 0.5f ;
	double y2 = y;

	b2BodyDef* defderecho = new b2BodyDef();
	defderecho->position.Set(x1, y1);
	defderecho->type = b2_dynamicBody;
	this->extremoDerecho = world->CreateBody(defderecho);
	b2PolygonShape caja;
	caja.SetAsBox(0.5f, 0.5f);
	this->extremoDerecho->CreateFixture(&caja,0.0f);


	b2BodyDef* defizquierdo = new b2BodyDef();
	defizquierdo->type = b2_dynamicBody;
	defizquierdo->position.Set(x2, y2);

	this->extremoIzquierdo =  world->CreateBody(defizquierdo);
	this->extremoIzquierdo->CreateFixture(&caja,0.0f);

	b2RevoluteJointDef* jointDerDef = new b2RevoluteJointDef();
	jointDerDef->Initialize(this->cuerpoFisico,extremoDerecho,  extremoDerecho->GetPosition());
	world->CreateJoint(jointDerDef);

	b2RevoluteJointDef* jointDerDef2 = new b2RevoluteJointDef();
	jointDerDef2->Initialize(this->cuerpoFisico,extremoIzquierdo,  extremoIzquierdo->GetPosition());
	world->CreateJoint(jointDerDef2);




	// guardo informacion propia del carrito

	this->guardarInformacionDeEntidad();
	this->alto = alto;
	this->ancho = ancho;

}

RuedaFisica* CarritoFisica::getRuedaIzquierda(){
	return this->ruedaIzq;
}

RuedaFisica* CarritoFisica::getRuedaDerecha(){
	return this->ruedaDer;
}

void CarritoFisica::aplicarFuerzasFisicas(){
//	cout<<"x , y CARROOOO: "<<this->extremoIzquierdo->GetPosition().x<<"; "<<this->extremoIzquierdo->GetPosition().y<<endl;
//	cout<<"x , y CARROOOO: "<<this->extremoDerecho->GetPosition().x<<"; "<<this->extremoDerecho->GetPosition().y<<endl;
}

b2Body* CarritoFisica::getExtremoDerecho(){
	return this->extremoDerecho;
}

b2Body* CarritoFisica::getExtremoIzquierdo(){
	return this->extremoIzquierdo;
}

CarritoFisica::~CarritoFisica() {
	// TODO Auto-generated destructor stub
}

